﻿using System;
using YJKGeometryFuncs;

namespace YJKLoopSearch.LoopSearch
{
	public class KeyPoint
	{
		public KeyPoint(KeyPoint pt)
		{
			this.m_X = pt.m_X;
			this.m_Y = pt.m_Y;
			this.m_Z = pt.m_Z;
		}

		public KeyPoint(double x, double y, double z)
		{
			this.m_X = x;
			this.m_Y = y;
			this.m_Z = z;
		}

		public static KeyPoint operator -(KeyPoint source)
		{
			return new KeyPoint(-source.X, -source.Y, -source.Z);
		}

		public static KeyPoint operator -(KeyPoint left, KeyPoint right)
		{
			return new KeyPoint(left.X - right.X, left.Y - right.Y, left.Z - right.Z);
		}

		public static KeyPoint operator *(double value, KeyPoint right)
		{
			return new KeyPoint(right.X * value, right.Y * value, right.Z * value);
		}

		public static KeyPoint operator *(KeyPoint left, double value)
		{
			return new KeyPoint(left.X * value, left.Y * value, left.Z * value);
		}

		public static KeyPoint operator /(KeyPoint left, double value)
		{
			return new KeyPoint(left.X / value, left.Y / value, left.Z / value);
		}

		public static KeyPoint operator +(KeyPoint left, KeyPoint right)
		{
			return new KeyPoint(left.X + right.X, left.Y + right.Y, left.Z + right.Z);
		}

		public double X
		{
			get
			{
				return this.m_X;
			}
		}

		public double Y
		{
			get
			{
				return this.m_Y;
			}
		}

		public double Z
		{
			get
			{
				return this.m_Z;
			}
		}

		public double AngleOnPlaneTo(KeyPoint right, KeyPoint normal)
		{
			double num = this.X * right.X + this.Y * right.Y;
			double num2 = Math.Sqrt(Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0)) * Math.Sqrt(Math.Pow(right.X, 2.0) + Math.Pow(right.Y, 2.0));
			double num3 = Math.Acos(num / num2);
			if (Geometry.LessThan(Math.Atan(this.X / this.Y), Math.Atan(right.X / right.Y)))
			{
				num3 = Math.PI*2 - num3;
			}
			return num3;
		}

		public double DistanceTo(KeyPoint source)
		{
			return Math.Sqrt(Math.Pow(this.X - source.X, 2.0) + Math.Pow(this.Y - source.Y, 2.0));
		}

		public double GetLength()
		{
			return Math.Sqrt(Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0));
		}

		public bool IsAlmostEqualTo(KeyPoint source)
		{
			return Geometry.IsEqual(this.X, source.X) && Geometry.IsEqual(this.Y, source.Y) && Geometry.IsEqual(this.Z, source.Z);
		}

		public bool IsAlmostEqualTo(KeyPoint source, double tolerance)
		{
			return Geometry.IsEqual(this.X, source.X, tolerance) && Geometry.IsEqual(this.Y, source.Y, tolerance) && Geometry.IsEqual(this.Z, source.Z, tolerance);
		}

		public KeyPoint Normalize()
		{
			double length = this.GetLength();
			double x = this.X / length;
			double y = this.Y / length;
			return new KeyPoint(x, y, 0.0);
		}

		private double m_X;

		private double m_Y;

		private double m_Z;
	}
}
